/*
 * line_following.h
 *
 *  Created on: May 28, 2011
 *      Author: minyaung
 */

#ifndef LINE_FOLLOWING_H_
#define LINE_FOLLOWING_H_

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/core/core.hpp>
#include <std_msgs/String.h>
#include <geometry_msgs/Pose2D.h>
#include <tf/transform_listener.h>
#include <math.h>

#define PI 3.14159265

#define DEBUG

namespace enc = sensor_msgs::image_encodings;

#ifdef DEBUG
//We only want to display locally in debug mode
static const char WINDOW1[] = "Blue Image Mask";
static const char WINDOW4[] = "Sobel Image";
static const char WINDOW2[] = "Masked Filter Output";
static const char WINDOW3[] = "Line Filter Output";
#endif

class line_following {
private:
	ros::NodeHandle lfNodeHandle;
	image_transport::ImageTransport transport;
	image_transport::Subscriber color_sub;
	ros::Publisher lf_publisher;
	ros::Publisher line_publisher;
	ros::Subscriber nav_subscriber;
	tf::TransformListener listener;
	cv::Mat colorEdges[3];

	unsigned int kSize;
	unsigned int queueSizes;
	unsigned int xPosition;
	unsigned int yPosition;
	double facingAngle;
	double sensorHeight;
	std::string seekerName;
	unsigned int pixelToInch;
	unsigned int lowThreshold;
	//std::vector<int> detectedBreaks;
	//std::vector<int> filteredBreaks;

public:
	line_following();

	~line_following();

	void convolute(const sensor_msgs::ImageConstPtr& msg);
	double computeBreakDistance(int index, int rows);
	void poseCallback(const geometry_msgs::PoseStampedConstPtr& msg);

	void print(cv::Mat& mat, std::string name);
	void print(std::vector<int>& vec, std::string name);
	void print(std::vector<char>& vec, std::string name);
};

#endif //LINE_FOLLOWING_H
